/**
 * @file SimpleActionClientD.h
 * @author  
 * @brief 
 * @version 1.0
 * @date 2021-07-1
 * 
 * @copyright Copyright (c) 2021
 * 
 */

#pragma once

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

enum class ActionStatus
{
    UNKNOWN = 0,
    PROCESSING = 1,
    FAILED = 2,
    SUCCEEDED = 3
};
template<typename ActionT>
class SimpleActionClientD
{
    using SendGoalOptions = typename rclcpp_action::Client<ActionT>::SendGoalOptions;
    public :
        SimpleActionClientD(const rclcpp::Node::SharedPtr &ros_node,const std::string action_name)
        {
            ros_node_=ros_node;
            nav_to_pose_client_ = rclcpp_action::create_client<ActionT>(ros_node_, action_name);
            send_goal_options = SendGoalOptions();
            send_goal_options.result_callback =
                            std::bind(&SimpleActionClientD::resultCallback, this, std::placeholders::_1);
            send_goal_options.goal_response_callback =
                            std::bind(&SimpleActionClientD::goalResponseCallback, this, std::placeholders::_1);
        };

        void resultCallback(const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result)
        {
            switch (result.code) {
                case rclcpp_action::ResultCode::SUCCEEDED:
                    current_goal_status_ = ActionStatus::SUCCEEDED;
                    action_result=result.result;
                    break;
                case rclcpp_action::ResultCode::ABORTED:
                    current_goal_status_ = ActionStatus::FAILED;
                    break;
                case rclcpp_action::ResultCode::CANCELED:
                    current_goal_status_ = ActionStatus::FAILED;
                    break;
                default:
                    current_goal_status_ = ActionStatus::UNKNOWN;

                std::cout<< "resultCallback current_goal_status_= " << (int)current_goal_status_<< std::endl;
                return;
            }
        };

        void goalResponseCallback(const typename rclcpp_action::ClientGoalHandle<ActionT>::SharedPtr &future)
        {
            auto goal_handle = future.get();
            if (!goal_handle) 
            {
                current_goal_status_ = ActionStatus::FAILED;
                std::cout<< "action client failed to send goal to server."<< std::endl;
                return;
            }
            std::cout<< "action client success to send goal to server."<< std::endl;
        };

        std::shared_ptr<typename ActionT::Result> sendGoal(typename ActionT::Goal & pose_goal)
        {
            client_goal =pose_goal;
            return work();
        };


        void sendGoalBackThread(typename ActionT::Goal & pose_goal)
        {
            client_goal =pose_goal;
            std::thread{ std::bind(&SimpleActionClientD::work,this)}.detach();
        };

        std::shared_ptr<typename ActionT::Result> work()
        {
            rclcpp::Rate r(20);
            nav_to_pose_client_->async_send_goal(client_goal, send_goal_options);
            current_goal_status_ = ActionStatus::PROCESSING;
            while(rclcpp::ok())
            {
                
                if (is_cancel_requested) 
                {
                    auto cancel_future = nav_to_pose_client_->async_cancel_all_goals();
                    return nullptr;
                }

                if (current_goal_status_ == ActionStatus::FAILED) 
                {
                    RCLCPP_WARN(ros_node_->get_logger(), "Failed !");
                    return nullptr;
                } 
                else if (current_goal_status_ == ActionStatus::SUCCEEDED) 
                {
                    RCLCPP_INFO(ros_node_->get_logger(), "Succeeded !");
                    return action_result;
                }
                //rclcpp::spin_some(ros_node_);
                r.sleep();
            }
            std::cout << "----------- work() ------------" << std::endl;
        };

        bool isFinish() {return finished;};

        void setFinish(bool finished_) {finished=finished_;};

        void setState(ActionStatus  client_goal_)  {current_goal_status_ = client_goal_;};

        ActionStatus  runState() { return current_goal_status_;};
        bool wait_server(int second)
        {
            return nav_to_pose_client_->wait_for_action_server(std::chrono::seconds(second));
        }

    public:    
        rclcpp::Node::SharedPtr ros_node_;
        typename rclcpp_action::Client<ActionT>::SharedPtr nav_to_pose_client_;
        SendGoalOptions  send_goal_options;
        typename ActionT::Goal client_goal;
         std::shared_ptr<typename ActionT::Result> action_result=nullptr;

        bool finished=false;
        ActionStatus current_goal_status_= ActionStatus::UNKNOWN;
        bool is_cancel_requested=false;      
};
